/*
 * IO_card.c
 *
 * Created: 11.10.2012 09:52:17
 *  Author: bardbakk
 */ 
#define F_CPU 8500000
#define F_CPU_DIV1024 8300

#include <avr/interrupt.h>
#include <avr/io.h>
#include <avr/delay.h>
#include <stdlib.h>

#include "definitions.h"
#include "CAN.h"
#include "SERVO.h"
#include "MOTOR.h"
#include "REG.h"
#include "SOLENOID.h"
#include "definitions.h"

typedef enum {INIT, PLAY, IDLE}modes;

int main(void)
{		
	CAN_message_t *msg = NULL;
	CAN_message_t *recv_msg = calloc(1, sizeof(CAN_message_t));
	CAN_message_t send_msg;
	
	uint8_t readyToShoot = FALSE;
	uint16_t prevTimer = 0;
	float dt = 0;
	modes mode = INIT;

	//Init timer3 with 1/1024 prescaler
	BIT_SET(TCCR3B,CS30);
	BIT_SET(TCCR3B,CS31);

	sei();
    while(1)
    {	
		if(CAN_msg_rcvd)
		{
			msg = CAN_message_receive();
			memcpy(recv_msg, msg, sizeof(CAN_message_t));
			free(msg);			
		}
		
		switch(recv_msg->id)
		{
			case CAN_CHANGE_PI_MSG:
				REG_param_save(msg->data[0], msg->data[1]);
				break;
			case CAN_REQUEST_PI_MSG:
				send_msg.data[0] = REG_read_P();
				send_msg.data[1] = REG_read_D();
				send_msg.length = 2;
				send_msg.id = CAN_READ_PI_EEPROM_MSG;
				CAN_message_send(send_msg);
				break;
			case CAN_PLAY_MSG:
				mode = PLAY;
				break;
			case CAN_PAUSE_PLAY_MSG:
				mode = IDLE;
				break;								
		}
		
		switch(mode)
		{
			case INIT:
				ADC_init();
				DAC_init();
				SERVO_PWM_init();
				MOTOR_init();
				SOLENOID_init();
				CAN_init();
				
				//Send PI values
				send_msg.data[0] = REG_read_P();
				send_msg.data[1] = REG_read_D();
				send_msg.length = 2;
				send_msg.id = CAN_READ_PI_EEPROM_MSG;
				CAN_message_send(send_msg);
				
				mode = IDLE;
				break;
				
			case PLAY:
				if((recv_msg->id == CAN_CONTROL_MSG) || (recv_msg->id == CAN_TRACK_CONTROL_MSG))
				{
					
					//Set position reference
					MOTOR_set_reference(recv_msg->data[0]);
					SERVO_set_position(recv_msg->data[1]);
					
					//Check if it's time to shoot, only once every 0.5 second
					if(TCNT3 > 8000)
					{
						readyToShoot = TRUE;
					}
					if(recv_msg->data[3] && readyToShoot)
					{
						SOLENOID_shoot();
						TCNT3 = 0;
						readyToShoot = FALSE;
					}
				}
	
				//Update PID Controller
				dt = (float)(TCNT3 - prevTimer);
				if(dt > 0)
				{
					dt = dt/F_CPU_DIV1024;
					MOTOR_update_input(dt);	
				}
				prevTimer = TCNT3;
				
				//Check if there's something obstructing the photodiode
				if(ADC_read()<10)
				{
					//Send goal msg
					send_msg.id = CAN_GOAL_MSG;
					send_msg.length = 0;
					CAN_message_send(send_msg);
					
					//Disable motor
					MOTOR_set_speed(0);
					
					mode = IDLE;
				}	
				break;
				
			case IDLE:
				break;
	
		}
		recv_msg->id = 255;
	}
	
}
